#include <cereal/archives/binary.hpp>
#include <cereal/archives/json.hpp>
#include <cereal/types/memory.hpp>
#include <cereal/types/unordered_map.hpp>
#include <gtest/gtest.h>
#include <struct_example.hpp>

TEST(TestStruct, InitTest) {
  Vec3f vec;
  EXPECT_FLOAT_EQ(vec.x, 0.0f);
  EXPECT_FLOAT_EQ(vec.y, 0.0f);
  EXPECT_FLOAT_EQ(vec.z, 0.0f);

  Pose3f pose1;
  pose1.quality = PoseQuality::Bad;
  EXPECT_FLOAT_EQ(pose1.orientation.w, 0.0f);

  Pose3f pose{
      .position{9, 99, 999},
      .orientation{
          .x = 1,
          .y = 2,
          .z = 3,
          .w = 4,
      },
      .quality{PoseQuality::Good},
  };
  EXPECT_FLOAT_EQ(pose.orientation.w, 4.0f);
  EXPECT_FLOAT_EQ(pose.position.y, 99.0f);
}

void vect3f_equal(const Vec3f &v1, const Vec3f &v2) {
  EXPECT_FLOAT_EQ(v1.x, v2.x);
  EXPECT_FLOAT_EQ(v1.y, v2.y);
  EXPECT_FLOAT_EQ(v1.z, v2.z);
}

void quat4f_equal(const Quat4f &v1, const Quat4f &v2) {
  EXPECT_FLOAT_EQ(v1.x, v2.x);
  EXPECT_FLOAT_EQ(v1.y, v2.y);
  EXPECT_FLOAT_EQ(v1.z, v2.z);
  EXPECT_FLOAT_EQ(v1.w, v2.w);
}

void pose3f_equal(const Pose3f &pose1, const Pose3f &pose2) {
  vect3f_equal(pose1.position, pose2.position);
  quat4f_equal(pose1.orientation, pose2.orientation);
  EXPECT_EQ(pose1.quality, pose2.quality);
}

TEST(TestStruct, JsonSerialization) {
  Pose3f pose1{
      .position{9.f, 99.f, 999.f},
      .orientation{
          .x = 1.f,
          .y = 2.f,
          .z = 3.f,
          .w = 4.f,
      },
      // 如果不初始化，就会出现无效值。
      .quality{PoseQuality::Bad},
  };
  Pose3f pose2{};
  pose2.quality = PoseQuality::Bad;
  std::map<int64_t, Pose3f> time_to_pose;
  time_to_pose[11] = pose1;
  time_to_pose[22] = pose2;
  pose1.position.z = 45;
  time_to_pose[33] = pose1;

  std::stringstream ss;
  {
    cereal::BinaryOutputArchive archive(ss);
    archive(time_to_pose);
  }
  ss.seekg(0);
  std::map<int64_t, Pose3f> time_to_pose2;
  {
    cereal::BinaryInputArchive archive(ss);
    archive(time_to_pose2);
  }
  EXPECT_EQ(time_to_pose2.size(), 3);
  for (const auto [key, pose1] : time_to_pose) {
    EXPECT_TRUE(time_to_pose2.count(key) == 1);
    const auto &pose2 = time_to_pose2[key];
    pose3f_equal(pose1, pose2);
  }
}